/*******************************************************************************
* This file implements the MAVLink to communicate with the groundstation.
*
* Author: Kong Wai Weng @ Cytron Technologies Sdn. Bhd.
*******************************************************************************/



#include <stdio.h>
#include "ep_mavlink.h"



/*******************************************************************************
* PRIVATE GLOBAL VARIABLES                                                     *
*******************************************************************************/

// Define the system information.
uint8_t system_id = 100;
uint8_t component_id = 200;
uint8_t type = MAV_TYPE_FIXED_WING;
uint8_t autopilot = MAV_AUTOPILOT_GENERIC;
uint8_t base_mode = MAV_MODE_FLAG_SAFETY_ARMED;
uint32_t custom_mode = 0;
uint8_t system_status = MAV_STATE_ACTIVE;

uint32_t onboard_control_sensors_present = 0xfc3f;
uint32_t onboard_control_sensors_enabled = 0xfc3f;
uint32_t onboard_control_sensors_health = 0xfc3f;

uint16_t load = 0;

 
// Initialize the required buffers.
mavlink_message_t msg;

// Microseconds since system boot.
uint64_t usec = 0;



/*******************************************************************************
* PRIVATE FUNCTION PROTOTYPES                                                  *
*******************************************************************************/

static void prv_vSendMsg(mavlink_message_t* msg);



/*******************************************************************************
* TASK: taskMavlinkHeartbeat
*
* DESCRIPTIONS:
* This task sends the heartbeat via MAVLink to indicate that it's alive.
*
*******************************************************************************/
portTASK_FUNCTION(taskMavlinkHeartbeat, pvParameters)
{
	unsigned int uiLoopCount = 0;
	
	// We need to initialise xLastFlashTime prior to the first call to vTaskDelayUntil().
	portTickType xLastFlashTime = xTaskGetTickCount();
	
	while (1) {
		// Read the sensor data.
		struct ATTITUDE xAttitude = xGetAttitude();
		struct GPS_DATA xGpsData = xGetGpsData();
                float fAirSpeed = fGetAirspeed();
                float fAltitude = fGetPressureAltitude();
                static float fPreviousAltitude = 0;
                float fClimbRate = 0;
                
			
//xGpsData.ucFixStatus = 1;		
//xGpsData.fLatitude = 5.265931f;
//xGpsData.fLongitude = 100.434799f;
//xGpsData.fAltitude = 100.0f;
//xGpsData.fGroundSpeed = 3.0f;
//xGpsData.fGroundCourse = 45.0f;
//xGpsData.fHdop = 10.0f;
//xGpsData.uiNumberOfSatelites = 8;


		
		// Increase the loop count.
		if (++uiLoopCount >= 100) {
			uiLoopCount = 0;
		}



                // Calculate the climb rate every second.
		if ((uiLoopCount % 5) == 0) {
                    fClimbRate = fAltitude - fPreviousAltitude;
                    fPreviousAltitude = fAltitude;
                }
		
		
		// #####################################################################
		// Mavlink Heartbeat (1 Hz).
		// #####################################################################
		
		// Send the message every second.
		if ((uiLoopCount % 5) == 0) {

                    // Determine the mode.
                    custom_mode = eGetControlMode();
                    
	
                    // Pack the message.
                    // mavlink_msg_heartbeat_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
                    //			       			  uint8_t type, uint8_t autopilot, uint8_t base_mode,
                    //							  uint32_t custom_mode, uint8_t system_status)
                    mavlink_msg_heartbeat_pack(system_id, component_id, &msg,
                                               type, autopilot, base_mode,
                                               custom_mode, system_status);

                    // Send the message.
                    prv_vSendMsg(&msg);
		}
		
		
		
		// #####################################################################
		// System Status (1 Hz).
		// #####################################################################
		
		// Send the message every second.
		if ((uiLoopCount % 5) == 0) {
			
			// Get the battery voltage and calculate the remaining capacity.
			uint16_t voltage_battery = uiReadVin();
                        int16_t current_battery = uiReadCurrent() / 10;
			
//			int8_t battery_remaining = (signed char)(((unsigned long)voltage_battery - 10000L) * 1000L / 26L);
			int8_t battery_remaining = 100;
	
			// Pack the message.
            // mavlink_msg_sys_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
			//                             uint32_t onboard_control_sensors_present, uint32_t onboard_control_sensors_enabled,
            //                             uint32_t onboard_control_sensors_health, uint16_t load,
            //                             uint16_t voltage_battery, int16_t current_battery, int8_t battery_remaining,
            //                             uint16_t drop_rate_comm, uint16_t errors_comm, uint16_t errors_count1,
            //                             uint16_t errors_count2, uint16_t errors_count3, uint16_t errors_count4)
			mavlink_msg_sys_status_pack(system_id, component_id, &msg,
			                             onboard_control_sensors_present, onboard_control_sensors_enabled,
                                         onboard_control_sensors_health, load,
                                         voltage_battery, current_battery, battery_remaining,
                                         0, 0, 0,
                                         0, 0, 0);
			
			// Send the message.
			prv_vSendMsg(&msg);
		}
		
		
		
//		// #####################################################################
//		// Raw IMU data (5 Hz).
//		// #####################################################################
//
//		// Read from accelerometer, gyro and magnetometer.
//		struct ACC_DATA_RAW xAccData = xReadAccRaw();
//		struct GYRO_DATA_RAW xGyroData = xReadGyroRaw();
//		struct MAG_DATA_RAW xMagData = xReadMagRaw();
//
//		// Pack the message.
//		// mavlink_msg_raw_imu_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t usec,
//		//							int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag)
//		mavlink_msg_raw_imu_pack(system_id, component_id, &msg, usec,
//								 xAccData.siAccX, xAccData.siAccY, xAccData.siAccZ,
//								 xGyroData.siGyroX, xGyroData.siGyroY, xGyroData.siGyroZ,
//								 xMagData.siMagX, xMagData.siMagY, xMagData.siMagZ);
//
//		// Send the message.
//		prv_vSendMsg(&msg);
		
		
		
		// #####################################################################
		// Attitude (5 Hz).
		// #####################################################################
		
		// Pack the message.
		// mavlink_msg_attitude_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t usec,
		//							 float roll, float pitch, float yaw,
		//							 float rollspeed, float pitchspeed, float yawspeed)
		mavlink_msg_attitude_pack(system_id, component_id, &msg, usec,
								  xAttitude.fRoll, xAttitude.fPitch, xAttitude.fYaw,
								  xAttitude.fRateX, xAttitude.fRateY, xAttitude.fRateZ);
		
		// Send the message.
		prv_vSendMsg(&msg);
		
		
		
		// #####################################################################
		// VFR HUD (5 Hz).
		// #####################################################################
		
		// Calculate the throttle.
                unsigned int uiThrottle;
                if (OC3RS > PULSE_WIDTH_1000) {
                    uiThrottle = (unsigned int)((unsigned long)(OC3RS - PULSE_WIDTH_1000) * 100L / (unsigned long)PULSE_WIDTH_1000);
                }
                else {
                    uiThrottle = 0;
                }

		// Pack the message.
		// mavlink_msg_vfr_hud_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
		//							float airspeed, float groundspeed, int16_t heading,
		//							uint16_t throttle, float alt, float climb)
		mavlink_msg_vfr_hud_pack(system_id, component_id, &msg,
								 fAirSpeed, xGpsData.fGroundSpeed, (int16_t)xGpsData.fGroundCourse,
								 uiThrottle, fAltitude, fClimbRate);
		
		// Send the message.
		prv_vSendMsg(&msg);
		
		
		
		// #####################################################################
		// GPS data and status (1 Hz).
		// #####################################################################

		// Send the message every 500ms.
		if ((uiLoopCount % 5) == 0) {


//			// Pack the message.
//			// mavlink_msg_gps_raw_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t usec,
//			//							uint8_t fix_type, float lat, float lon,
//			//							float alt, float eph, float epv, float v, float hdg)
//			mavlink_msg_gps_raw_pack(system_id, component_id, &msg, usec,
//									 (xGpsData.ucFixStatus == 1)? 3 : 0, xGpsData.fLatitude, xGpsData.fLongitude,
//									 xGpsData.fAltitude, xGpsData.fHdop / 10.0f, 1.0f, xGpsData.fGroundSpeed, xGpsData.fGroundCourse);

            // Pack the message.
            // mavlink_msg_gps_raw_int_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t time_usec,
            //                              uint8_t fix_type,
            //                              int32_t lat,
            //                              int32_t lon,
            //                              int32_t alt,
            //                              uint16_t eph, uint16_t epv,
            //                              uint16_t vel, uint16_t cog,
            //                              uint8_t satellites_visible)
            mavlink_msg_gps_raw_int_pack(system_id, component_id, &msg, usec,
                                         (xGpsData.ucFixStatus == 1)? 3 : 0,
                                         (int32_t)(xGpsData.fLatitude * 10000000.0f),
                                         (int32_t) (xGpsData.fLongitude * 10000000.0f),
                                         (int32_t)(xGpsData.fAltitude * 1000.0f),
                                         65535, 65535,
                                         (uint16_t)xGpsData.fGroundSpeed * 100.0f,
                                         (uint16_t)xGpsData.fGroundCourse * 100.0f,
                                         xGpsData.uiNumberOfSatelites);



			// Send the message.
			prv_vSendMsg(&msg);



//			uint8_t cDummy[20] = {0};
//			// Pack the message.
//			// mavlink_msg_gps_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
//			//							   uint8_t satellites_visible, const int8_t* satellite_prn,
//			//							   const int8_t* satellite_used, const int8_t* satellite_elevation,
//			//							   const int8_t* satellite_azimuth, const int8_t* satellite_snr)
//			mavlink_msg_gps_status_pack(system_id, component_id, &msg,
//										xGpsData.uiNumberOfSatelites, cDummy,
//										cDummy, cDummy,
//										cDummy, cDummy);
//
//			// Send the message.
//			prv_vSendMsg(&msg);

		}



		// #####################################################################
		// #####################################################################
		
		
		
		// Record the stack high water mark.
		xStackWaterMark.uiMavlinkHeartbeat = uxTaskGetStackHighWaterMark(NULL);
		
		// Set the error flag if the water mark is too low.
		if (xStackWaterMark.uiMavlinkHeartbeat < RTOS_STACK_THRESHOLD) {
			xSystemError.bRtosStackError = 1;
		}
		
		// Delay for 200ms (5Hz).
		vTaskDelayUntil(&xLastFlashTime, configTICK_RATE_HZ/5);
		
		// Increase the timestamp.
		usec += 200000;
	}	
}



/*******************************************************************************
* PRIVATE FUNCTION: prv_vSendMsg
*
* PARAMETERS:
* ~ msg		- Mavlink message.
*
* RETURN:
* ~ void
*
* DESCRIPTION:
* Copy the message to the send buffer and send the message via UART.
*
*******************************************************************************/
static void prv_vSendMsg(mavlink_message_t* msg)
{
	static uint8_t ucBuffer[MAVLINK_MAX_PACKET_LEN];
	
	// Copy the message to the send buffer and send the message.
	uint16_t uiLength = mavlink_msg_to_send_buffer(ucBuffer, msg);
	if (ucGetSendToPcSpace() > uiLength) {
		ucSendToPc(ucBuffer, uiLength);
	}
}	
